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^ Abstract 

Uln this paper we provide an optimal control based strategy to explore feasible trajectories of nonlinear systems, that is 
to find curves that satisfy the dynamics as well as point- wise state-input constraints. The strategy is interesting itself in 
understanding the capabilities of the system in its operating region, and represents a preliminary tool to perform trajectory 
• tracking in presence of constraints. The strategy relies on three main tools: dynamic embedding, constraints relaxation and 
novel optimization techniques, introduced in [10,12], to find regularized solutions for point-wise constrained optimal control 
^ problems. The strategy is applied to the PVTOL, a simplified model of a real aircraft that captures the main features and 
g challenges of several "maneuvering systems" . 
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1 Introduction 

(N 

^-H In several fields as aerospace, robotics and automotive, designers have to deal with complex nonlinear system dy- 
^-H namics. A deep knowledge of the behavior of such systems is fundamental both in controlling them about a (possibly 
aggressive) desired trajectory and in assisting the engineer in the design process. An interesting problem is, therefore, 
^ the exploration of the trajectory manifold of the system, that is, the characterization of the system (state-input) 
trajectories and their parametrization with respect to " performance-output curves". More formally, given a desired 
/\ curve for some of the states (outputs), we aim at finding a state-input lifted trajectory (i.e. a state- input curve 
^ satisfying the dynamics) whose outputs are close to the desired ones. The solution of this problem is interesting 
itself in understanding the behavior of the system and provides a nominal trajectory that can be used in a receding 
horizon scheme for trajectory tracking. 



Having in mind engineering applications, there is an important aspect to take into account in the solution of 
exploration and tracking, that is the presence of constraints in the system. Such constraints may arise from diverse 
causes such as: physical bounds on the states and the inputs, validity bounds of the model or presence (respectively 
absence) of important properties (e.g. controllability). We will call the region where constraints are satisfied feasibility 
region. This implies that, not only we look for lifted trajectories (curves satisfying the dynamics), but furthermore 
we ask for feasible lifted trajectories, that is trajectories lying in the feasibility region. 

In this paper we concentrate our attention on a special class of nonlinear systems that we call maneuvering systems. 
In this class we include those systems for which a natural notion of performance outputs is present. Namely, some 
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of the states are requested to follow (almost exactly) a desired profile, while the remaining states meet suitable 
(feasibility) bounds. Maneuvering systems include for example vehicles (cars, motorcycles, aerial vehicles, marine 
vehicles), manipulators and several other mechanical systems. In this paper we consider as "prototype example" of 
maneuvering system the PVTOL aircraft. The PVTOL was introduced by Hauser et al. in [15] in order to capture 
the lateral non-minimum phase behavior of a Vertical Take Off and Landing (VTOL) aircraft. This model has been 
widely studied in the literature for its property of combining important features of nonlinear systems with "tractable" 
equations. Furthermore, the dynamics of many other mechanical (maneuvering) systems can be rewritten in a similar 
fashion, e.g., the cart-pole system, the pendubot [30], the bicycle model [9], [13] and the longitudinal dynamics of 
a real aircraft. Since the PVTOL has been introduced in 1992, many researchers have studied this system. A non 
exhaustive literature review of works on trajectory tracking or path following of the PVTOL includes [15,29,1,21,5,4]. 

The trajectory exploration problem has been investigated in the literature in different formulations. Trajectory explo- 
ration for a class of VTOL aircrafts is tackled in [24] , where an optimization strategy is proposed to compute optimal 
transition maneuvers. The problem of finding a (state-input) trajectory whose output is exactly an assigned desired 
curve is known in the literature as nonlinear inversion. This problem is particularly challenging for nonminimum- 
phase systems. The problem was introduced and solved for some classes of nonlinear systems and desired output 
curves in [7] and extended to time- varying and non- hyperbolic systems respectively in [8] and [6] . More recently, [28] , 
a new approach based on the notion of convergent systems was proposed to solve the problem. In [14] the nonlinear 
inversion problem was solved for an inverted pendulum by use of exponential dichotomy under mild conditions on 
the output curve. Nonlinear inversion is strongly related to the output regulation problem, that is to the design 
of a control law such that the system output asymptotically tracks a desired output curve. An early reference is 
[19]. There, the nonlinear inversion problem was solved by means of a suitable partial differential equation when the 
desired output curve is the trajectory of an exosystem. In [17] a two-step strategy was proposed to solve the stable 
inversion problem. An overview on the topic can be found in [3], whereas more recent references include [20,16,27]. 

The contribution of the paper is threefold. First, we propose an optimal control based strategy to compute feasible 
trajectories of maneuvering systems. The strategy relies on three main ideas: dynamic embedding, constraints relax- 
ation and continuation with respect to (the embedding and relaxation) parameters. In detail, we compute feasible 
trajectories (i.e., trajectories satisfying pointwise state and input constraints) that minimize a weighted L2 distance 
from the desired output curve. In order to compute an approximate solution to this problem we perform the fol- 
lowing steps. We embed the system into a family of systems and relax the feasibility region so that the constraints 
are not active. For each value of the system (embedding) and constraint (relaxation) parameters, we compute an 
unconstrained lifted (state-input) trajectory (by applying an optimal control based dynamic embedding technique) 
and use it as desired curve for a constrained L2 distance minimization. To compute a feasible trajectory (L2 close to 
the unconstrained one), we design a relaxed version of the constrained optimal control problem. The relaxation is 
based on the introduction of a parametrized barrier functional to handle the constraints [12]. The resulting optimal 
control problem is solved by means of a projection operator based Newton method [10]. The final ingredient of the 
strategy is a continuation procedure to update the embedding and relaxation parameters up to their nominal values. 

Second, we prove the effectiveness of the strategy, namely that a feasible lifted trajectory can be computed, for 
suitable values of the embedding and relaxation parameters. The proof of this result relies on the continuity and 
differentiability of an optimal control minimizer with respect to parameters, which is provided as a stand alone result. 
An analogous result was already proven in [22] for unconstrained systems and extended to input-state constrained 
systems in [23] . The main differences with the existing results are the following. We do not consider input and state 
constraints directly, but take them into account in a relaxed version of the constrained optimal control problem by 
use of a barrier functional, the barrier functional being weighted by one of the varying parameters. Furthermore, 
we take into account the system dynamics by means of a trajectory tracking projection operator. The projection 
operator is the key distinctive feature for the proof of the differentiability result. Indeed, the projection operator 
allows to convert the dynamically constrained optimal control problem into an unconstrained trajectory optimization 
problem. Thus, an appropriate implicit function theorem can be used to solve the first order necessary condition 
equation. The implicit function theorem allows to show that, if the second derivative of the cost composed with the 
projection operator is invert ible at the nominal parameter, then there is a neighborhood on which the local minimizer 
exists and is with respect to the parameter. It turns out that the appropriate condition to ensure invertibility of 
the operator is that the minimizer satisfies the second order sufficiency condition at the nominal parameter. 

Third and final, we provide a complete characterization of the exploration strategy for the PVTOL. In detail, we first 
solve the nonlinear inversion problem for the unconstrained PVTOL. Given any desired output curve resulting 
into a bounded acceleration profile, we prove that a trajectory can be computed for the decoupled system and for 
suitable positive values of the coupling parameter. Based on this result and on the second set of contributions. 
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we show that ah the strategy steps can be performed for suitable values of the feasibility region and the coupling 
parameter. Finally, we perform a numerical analysis showing that, in fact, feasible trajectories of the PVTOL can 
be computed for aggressive desired output curves even in presence of relatively tight constraints. 

The paper is organized as follows. In Section 2 the notion of maneuvering systems is introduced and the PVTOL 
aircraft is presented as a prototype example. Section 3 defines the performance tasks solved in the paper, namely 
unconstrained and constrained trajectory lifting. In Section 4 the unconstrained lifting task is solved for the PVTOL 
aircraft. That is, a lifted trajectory is proven to exists for some positive values of the coupling parameter. In Section 5 
the proposed optimal control based exploration strategy is presented and in Section 6 a theoretical analysis of the 
strategy is developed, proving that for suitable values of the system and constraint parameters a feasible trajectory 
exists. In Section 7 numerical computations are provided showing the effectiveness of the strategy for the constrained 
PVTOL on an aggressive barrel roll trajectory in presence of respectively input and state-input constraints. Finally, 
in Appendix A an overview of the trajectory tracking projection operator theory and the projection operator based 
Newton method for unconstrained and constrained optimal control problems is given. 



Notation For a function g : [0,T] ^W,T > 0, we let ||^(')lli/oo ~ ^^P^g[o,t] II5'(^)II be the usual norm. Let 
C^[0, T]P, Loo[0, TY and Lsp, T]p be the spaces of functions g : [0, T] W that are respectively k times differentiable 
with continuous k-ih. derivative, bounded and Lebesgue integrable, and square integrable on [0,T]. In the rest of the 
paper we will abuse notation and denote them as C^, L^o and L2 when domain and codomain are clear. Let ^ ^ ^(0 
be a twice Frechet differentiable operator, we denote respectively ^ DA{(^q) • ( and (C,^) ^ ^^^(^o) • iC^v) the 
first and second Frechet differentials of A at ^o- Given a control system x = f{x^u), where x G is the state and 
u G is the control input, we say that a bounded curve 77 = {x{-)^u{-)) is a (state-control) trajectory of the system 
if x{t) = f{x{t)^u{t)) for all t G [0,T], < T < +00, and x{0) = x{0). Trajectories of the system through xq belong 
to the affine subspace X := (xq, 0) + X^o, where X^o is the closed subspace of L^^[0, T] of curves ( = u) with 
continuous /3, /3(0) = 0, and bounded u. We denote T C X the set of bounded (in Loo) trajectories through xq. 

2 The PVTOL model and maneuvering system definition 

In this section we introduce the system that motivates our work, the PVTOL, and inspired by this model we 
introduce the notion of maneuvering systems. The PVTOL aircraft was introduced in [15]. Using standard aeronautic 
conventions the equations of motion are given by 

y = Ui sin cp — Cp U2 cos cp 

z = —UiCOS(p — epU2sm(p-\-g (1) 

(p = U2. 

The aircraft state is given by the position (?/, z) of the center of gravity, the roll angle (p and the respective velocities 
y^ z and (p. The control inputs ui and U2 are respectively the vertical thrust force and the rolling moment. The 
gravity acceleration is denoted by g. An interesting feature of the PVTOL model is that the rolling moment U2 
generates also a lateral force ep U2-, where ep G M is a coupling coefficient. In Figure 1 the PVTOL aircraft with the 
reference system and the inputs is shown. Depending on the value of ep the PVTOL shows very diverse dynamic 




Fig. 1. PVTOL aircraft. 

behaviors and possesses different control properties. We will clarify them in the next sections. 



3 



Next, we exploit an important feature of the PVTOL that can be generalized to a wide class of systems that we 
will refer to as maneuvering systems. In analyzing the PVTOL, we can partition the state space into the cartesian 
product of: i) external position states {y and z), ii) internal position states (cp) and iii) velocities (^, z and cp). In 
many engineering applications the objective is to track a time parametrized curve described by the external position 
states while maintaining the internal position states bounded (the trajectories of the velocities being consistent with 
the related positions). Thus, a natural choice of outputs arises for these systems, namely the external position states. 
We call these outputs performance outputs meaning that a task for the system may be defined by assigning a desired 
curve for these states (desired performance outputs)]^ From now on, since we will be only dealing with performance 
outputs, we will refer to them as outputs. A state space model of a maneuvering system is given by 

x{t)=f{x{t),u{t)), 
y{t)=p{x{t)), tG[0,T],T>0, 

where x, u and y are respectively the state, the input and the performance output, / : x is assumed 

to be in both arguments, and p selects a subset of the states (e.g. the external position states for the PVTOL). 

For the PVTOL, a state space model can be obtained by posing x = (?/, y^ i, (p) and u = (iii, 1^2)- Also, a natural 
choice of performance outputs is given by the position of the center of gravity (?/, z), so that p{x) := (?/, z) = (xi, X2). 

3 Development of performance tasks 

In this section we identify important challenges that arise in studying the PVTOL capabilities and that can be 
generalized to maneuvering systems. These challenges will drive us in providing useful strategies to explore the 
dynamic capabilities of maneuvering systems. 

We start defining the first two tasks we are interested in. Informally, given a time-parametrized desired output curve, 
we want to find a (state-control) trajectory of the system, such that the output portion of the trajectory is close 
to the desired output curve. We will call this task trajectory lifting. We will also define an approximate version of 
such task, called practical trajectory lifting^ suitable for computations. More formally we can define the two tasks as 
follows. Given an output curve a(-) G I/oo[0,T]^, we denote ||q^(-)IU2 ^ suitable weighted L2 norm of a(-), that is, 
l|c^(-)IU2 = Jq a{t)^Wa{t)dt ^ a{T)^Wia{T), with W and Wi positive definite matrices[£| 

Definition 3.1 (Trajectory lifting task) Lety^{t), t G [0,T]^ be a desired sufficiently smooth output curve. Find 
a bounded trajectory (x*(-), i^*(-)) G T such that 

Mx*{-)) - yd(-)llL < - yd(-)llL <^ii e r 

Definition 3.2 (Practical trajectory lifting task) Let yd{t), t G [0,T]^ be a desired sufficiently smooth output 
curve. For a given e > 0^ find a trajectory (xj(-), i^J(-)) G T such that 

lb«(-)) - yd(-)llL < - yd(-)llL + ^ M aii «(•)) e r 

Remark 3.3 (Trajectory lifting and dynamic inversion) Dynamic inversion, [7], is a trajectory lifting prob- 
lem in the case the time horizon is (—00, +00). The objective is to find a (state-input) trajectory such that the output 
trajectory is exactly the desired one. Clearly, if a solution to dynamic inversion exists, it is also a solution for the 
trajectory lifting task on the time horizon [0, T] with zero minimum cost. If such a trajectory exists we say that it 
exactly solves the trajectory lifting task. In the next section we will show that for the PVTOL it is in fact possible 
to solve the dynamic inversion problem. This could be not the case for other maneuvering systems, but (practical) 
trajectory lifting could still be solved by using optimal control. 

Remark 3.4 (Trajectory lifting for flat systems) In most cases the performance outputs are driven by the ap- 
plication and cannot be decided by the designer. Thus, even for systems that are feedback linearizable or differentially 



^ The performance outputs are different from measured outputs, i.e., states or functions of states that can be measured by a 
sensor. Measured outputs play an important role in control design, but will not be considered here. 
^ If = /n and Wi — this is the classical L2 norm. 
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flat, the design of a lifted trajectory is an issue. Regarding the PVTOL, in [15] it was shown that the decoupled 
PVTOL (ep = 0) was feedback linearizable, hence differentially flat, relative to the natural outputs. In [29] it was 
shown that the coupled PVTOL is also differentially flat, but with respect to the flat outputs = y -\- ep simp and 
Zf = z -\- ep cos (p. If the flat outputs were chosen as performance outputs, the problem of finding a trajectory of the 
system consistent with the outputs would be easily solved as for the decoupled model. However, physical considerations 
suggest that the natural outputs are better suited as performance outputs. 

In this paper we are interested in a more challenging task, namely a constrained version of the lifting task. That 
is, we want to perform the lifting task while enforcing point- wise constraints on control inputs and states. In other 
words, given a desired output curve and a region of the state-input space, we want to find a trajectory that lies 
entirely in the region and whose output portion is close (according to a given cost function) to the desired curve. 

More formally, we define a feasibility region XU C M'^ x as a a compact simply connected region of the state- 
input space where the trajectories of the system must lie at every time. Consistently, a feasible trajectory for XU is 
a trajectory of the system, G T, such that {x{t)^u{t)) G XU for almost all t G [0,T]. In the rest of the 

paper we will focus on trajectories that belong to the interior, XU^ of XIA. Thus, we say that a trajectory of the 
system, G T, is a strictly feasible trajectory for XU if {x{t)^u{t)) G XU for almost all t G [0,T]. We are 

now ready to define the constrained version of the lifting task. As for the unconstrained problem we define an exact 
and a practical task. 

Definition 3.5 (Feasible trajectory lifting task) Let yd{t), t G [0,T]^ be a desired sufficiently smooth output 
curve and XU C x a feasibility region. Find a feasible trajectory, (x*(-), ia*(-)) G T with {x^{t),u*{t)) G XU 
for almost all t G [0,T]^ such that 



for all {x{-),u{-)) G T with {x{t),u{t)) G XU for almost all t G [0,r]. 

Definition 3.6 (Practical feasible trajectory lifting task) Lety(\{t), t G [0,T]^ be a desired sufficiently smooth 
output curve and XU C x a feasibility region. For a given e> find a feasible trajectory, {xl{-)^ul{-)) G T 
with {xl(t),ul(t)) G XU for almost all t G [0,T], such that 



for all {x{-),u{-)) G T with {x{t),u{t)) G XU for almost all t G [0,T]. 

Finding a global solution to the above problems is a hard task since we are dealing with infinite dimensional 
optimization problems. Thus, our goal in this paper is to find a feasible trajectory that satisfies locally equation (2). 

4 Trajectory lifting for the unconstrained PVTOL 

In this section we solve the exact lifting task for the coupled PVTOL with positive values of the parameter Cp and 
show that the lifted trajectory depends continuously on the parameter. 

4-1 Trajectory lifting for the decoupled PVTOL model 

The exact lifting task can be easily solved for the decoupled PVTOL model, that is for the model with ep = 0. Since 
we will often refer to this special case, we use for it the ad hoc notation PVTOLq. In [15] the PVTOLq was shown to 
be input-output linearizable provided ui ^ 0. Here, we provide sufficient conditions to compute a trajectory of the 
system given a desired output curve. We rewrite the equation for the PVTOLq since this will play an important 
role in the development of our strategy. 



lb(a:*0)-yd(-)llL<lbW-))-ydOllL 



- yd(-)llL < - yd(-)llL + e 



(2) 



y = u\ sm p 



z = —u\ COS ^ ^ g 



(3) 



The following assumption will be used in the paper. 
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Assumption 4.1 (Annulus assumption) Let yd{-) G be a desired output curve. LetaLd{t) := {yd{t),g — ^di'^))'^ ^ 
assume that ad(t) 7^ and < amin < ||ad||<amin for all t. 

A graphical interpretation of the annulus assumption is depicted in Figure 2. We ask the vector aa to lie in the 
annulus of radiuses amin and amax centered at the origin of the reference axes y and z — g. 









1 C: 


z — a 


\ y 



Fig. 2. Acceleration range 

Under this assumption the (state-input) trajectory of the PVTOLq may be parametrized in terms of the desired 
output curve by 

(po{t) = Zad(t) = atan2(i/d(t),^ - Zd{t))) 
u,o{t) = lladWII = {{g-Zd{t)?^ym'^ 

Equations in (4) allow to compute a trajectory of the PVTOLq for a given output curve satisfying the annulus 
assumption, thus exactly solving the trajectory lifting task (on any interval [0,T]). This is a straightforward conse- 
quence of the input-output linearizability of PVTOLq . 

4.2 Trajectory lifting for the coupled PVTOL via dichotomy 

Next, we prove that, given a desired output curve satisfying the annulus assumption, it exists a trajectory of the 
PVTOL exactly solving the lifting task for suitable positive values of the parameter ep and, as ep goes to zero, this 
trajectory depends continuously on it. To prove the result we use a feedback transformation that takes the system 
into the form of a driven pushed pendulum where the parameter ep plays the role of the pendulum length. The result 
that we prove is based on and extends results in [14] on finding upright trajectories of an inverted pendulum. 

Let us consider the feedback transformation given by 

Ui 

ep U2 

The dynamics of the system becomes 

y = vi 

Z = V2 (6) 
^ = {g ~ ^2) sin (f — vi cos (p. 

We have written the dynamics in a form that is somehow unusual, since the parameter ep appears in the left hand 
side of the differential equation. This form has the advantage that it is well defined even for ep = 0. In this case the 
model in (6) is defined by an algebraic differential equation. Also, to be consistent, we have to consider as control 
inputs ui and ep U2 and think of the roll dynamics in (1) as ep (/3 = ep U2. 



sincp 


— cos (f 


( 


0" 




Vi 


coscp 


— s'mcp 




.-9. 







(5) 
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Remark 4.2 The feedback transformation highlights an important property of the coupled PVTOL (cp y^O), i.e., it 
has a well defined relative degree, r = [2,2]^ with respect to the output (y^z). It is worth noting that for ep > the 
zero dynamics of the system is unstable (the driven pendulum is pushed and thus inverted), and therefore the system 
is non- minimum phase. This is an interesting feature of the PVTOL that makes the lifting task more challenging. 

An important role in the study of the trajectory manifold of the PVTOL is played by the "quasi trajectory" that 
(with some abuse of notation) we call quasi-static trajectory. It is a time parametrized curve built pretending that, 
at each instant the roll angle assumes the equilibrium value obtained if y{t) and z{t) were constant. By imposing 
(f = in equation (6) we get 

It is worth noting that the quasi-static roll trajectory does not depend on ep and coincides with the roll trajectory 
that we obtained for the PVTOLq system. Also, in the driven pushed pendulum the quasi-static trajectory produces 
an acceleration vector aa aligned along the pendulum axis. 

A first straightforward but interesting result can be proven. Before stating the proposition we need some more 
notation. Recall that in Section 2 we have written the PVTOL dynamics in state space form and denoted x and u 
the state and the input of the system. Consistently with that notation we let x be the state of system in (6) and 
V := {vi^V2)^ so that x = /pend(^5'^) with suitably defined /pend- 

Proposition 4.3 Let yd(') be a desired output curve on (— oc, +oo) satisfying the annulus assumption. Let 

be the trajectory solving the exact lifting for the PVTOLq model in (3) according to (4) and (^pendl')? '^pend(')) 
the trajectory solving the exact lifting for the model in (6) with ep =0. Then 

= :r*gnd(') 

and 

'^i(-) = - ^d (•)) cos (/?*(-) + i/d(-) sin (/?*(•), 

consistently with equation (5). 

With this feedback transformation in hand, the exact lifting task for the PVTOL can be formulated as follows. Given 
a desired output curve yd{-)^ find a bounded roll trajectory for the roll dynamics 

ep ^ = - Zd{t)) sin LP - yd{t) cos (p. (8) 



The proof of existence of cp^^ (•) and right continuity with respect to ep is based on the presence of a dichotomy in 
the linearization of the dynamics of an inverted pendulum about the vertical position. In [14] a bounded trajectory 
of the inverted pendulum is proven to exist as a fixed point of a contraction mapping. Here, we generalize the result 
in [14] in the sense that we allow the acceleration of the pivot point to lie in the entire annulus (not only on the 
horizontal axis z = 0) and we study the properties of the lifted trajectory when the parameter ep (the length of the 
pendulum) goes to zero. 

We rewrite the roll dynamics in (8) in the form 

ep(f = ad{t) sin{(p - (pqs{t)), (9) 
where ad(t) = ||ad(t)||. Then we write it in terms of the error from the quasi-static angle, = (p — (/?qs, as 

ep = ad{t) sinO - ep (/3qs(t), (10) 
and, in order to use dichotomy, in terms of its linearization about ^ = 0, 

ep = ad{t)e - ad{t)(e - sm0 + ep ^^^^ (11) 
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Let us consider the linear time- varying system driven by a bounded external input 



(9-sin6> + ep 

^ ad(t) 



In [14] it was proven that, for any ep > 0, the undriven system admits an exponential dichotomy and, therefore that, 
working in a noncausal fashion, for any bounded input /i(-) a bounded solution 7(-) exists. We let Aep - L^^ L^^ 
be the linear map /i(-) i-^ 7(-)- The following holds. 

Lemma 4.4 (Theorem 5 in [14]) For any ep > 0; Aep is a bounded linear operator with \\Aep || = 1- 
Defining the nonlinear operator A4p as 

=:Mp [0{-)], 

Clearly, a bounded curve ^(•) is a solution of (10) if and only if it is a fixed point of A4p , i-e. ^(•) = A/'ep [0{-)]. 

We are now ready to prove the main result in this section. The proof relies on arguments of Theorem 8 in [14]. 

Theorem 4.5 Given a output curve (i/(-),z(-)) on (— (X),+(X)); with < amin < CLd{t) < amax for all t ((pqs{-) 
hounded), then there exists an epo > 0^ 

1 

~ ll^qs(-)/«d(-)l|L.' 

such that for any ep G (0, epo ) 

(i) there exists a hounded trajectory O^p (•) of (10) so that ip^^ {') = ^qs{') + ^ep (•) is a hounded trajectory of (8) 

whenever if qs{-) is bounded, and 
(ii) llv^ep (•)-M-)IUo.<sin-'^. 

Proof: In order to prove existence of the trajectory, we show that there exists > such that for any 5 G (0, 5q) 
the map M^p is a contraction on the invariant set Bs = {0{') G ||^(')||loo ^ The set is invariant if for 
any 5 

«d(-) 

Recall that \\Aep \\ = 1 for all ep > and f{5) = (5 — sin (5 is monotonically increasing on [0,7r]. Therefore, posing 
^ ll^qs(-)A(-)lkoo ' ^ ^ [^'^]' invariant under A/'ep if 



WKp [OmL^ < Mep llll^(-) -sin^(.)||L. +ep II^IIl. < S. 



< smo. 



Now, the function /(•) is Lipschitz continuous on [0, ^o] ^ [0? ^] with Lipschitz constant 1 — cos(5o, i.e. \f{Si) — f{S2)\ < 
(1 — cos ^o) 1^1 ~ ^2! for all ^1, ^2 ^ [O5 ^0] C [0,7r]. Therefore, choosing S < 7t/2 and such that sin^ > we have 



[^i(-)] -Mp M-Ml^ < p\\Oi{-)-02{-)\\l^, 

with p = 1 — cos ^ < 1, so that A4p is a contraction on the invariant set Bs- In particular, the minimal set is obtained 
for S = sin~^ This gives the bound 

epo ^ 

-1 



ll^(-)l|L^ = lbep (•)-^qs(-)l|L^<sin- 



proving statement (ii). 



Next theorem follows easily from the results above. 
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Theorem 4.6 (Exact lifting for the PVTOL) Lety^{t), t e [0,T], be a desired output curve for the PVTOL. 

Then there exists a trajectory with initial condition xq = x*(0); such that p{x''{t)) = yd(^) for all 

te[0,T]. 

Proof: The external velocities are easily the derivatives of the desired outputs, while the roll and roll rate trajectories 
0*(-) and on [0,T] can be chosen as the restriction to [0,T] of the trajectories on the infinite horizon. These 

are proven to exist by Theorem 4.5. ■ 



5 Optimal control based strategy for feasible trajectory lifting 

In this section we provide a strategy to solve the practical feasible trajectory lifting task. The idea is to attack the 
problem by means of optimal control combined with continuation and relaxation methods. 

Let ^ = {x{t)^u{t))^ t G [0,T], be a state-input curve. If {x{t)^u{t)) G XU for almost all t G [0,T], we say that ^ is 
a feasible curve and write ^ G XU. Proceeding formally, to solve the practical feasible lifting task in Definition 3.5, 
we should solve the following optimal control problem 

min \\p{x{-))-ya{')\\L^ 

subj. to^ G 'XU. 

The main idea behind our strategy is not to attack the constrained lifting problem directly, but to embed it into 
a family of relaxed problems and use continuation with respect to parameters to find a solution. Informally, we 
perform the following steps: (i) embed the maneuvering system into a family of systems, (ii) design a routine to solve 
the unconstrained lifting task for a fixed embedding system in order to obtain a desired (state-input) trajectory 
^d, (iii) minimize a weighted L2 distance from the infeasible unconstrained lifted trajectory, ^d, over the feasible 
trajectories, (iv) relax the constrained optimal control problem and design a solver for the relaxed problem (for each 
fixed embedding system), and (v) design an update policy for the system and problem parameters. 

Embedding systems 

We embed the maneuvering system into a family of systems with the property that, for some choice of the parameters, 
the system has a "special" structure. That is, there exist values of the parameters for which the (unconstrained) 
lifting task can be solved more easily (e.g. because the system is differentially fiat or has some "nice" geometry). 
As regards the PVTOL, we consider the family of PVTOL models parametrized by the coupling parameter ep with 
ep > 0. From the results in the previous section we know that we can easily solve the unconstrained lifting task 
for the decoupled system, i.e. for ep = 0, and that an unconstrained trajectory is proved to exist for some positive 
values of the parameter. 

Unconstrained lifting 

The objective of this step is to obtain a (state-input) trajectory solving the unconstrained lifting task to use as the 
desired curve for the constrained problem. We use a dynamic embedding technique introduced in [13]. Informally, it 
consists of embedding the original system into a fully controllable system and solve the practical trajectory lifting 
by penalizing the embedding input much more than the real ones. We describe an ad-hoc version of this technique 
for the PVTOL, but it can be easily generalized to other maneuvering systems. 

First, observe that for ep = a lifted trajectory of the PVTOL can be easily obtained by equations in (4). For 
ep > we know by Theorem 4.5 that, under suitable conditions on yd{-)^ there exists a trajectory (on the infinite 
time horizon) solving the exact lifting task, and it depends continuously on ep . An approximation of this trajectory 
on the finite horizon can be computed in three steps: (i) compute the external position and velocities trajectories, (ii) 
compute the internal trajectories, (iii) compute the input trajectories. The first step is straightforward. The external 
position trajectories are simply given by the desired curves and the velocities are obtained by differentiation. The 
input trajectories can be easily computed by equation (5) once the trajectory of the internal position state (the roll 
trajectory) is known. Thus, the dynamic embedding technique is applied to compute the roll and roll rate trajectories. 
We use the dynamics in the new coordinates (6) and find a trajectory of the reduced system in (8). We embed the 
roll dynamics into the driven system 

^p^= {9- ^d{t)) simp- yd{t) cos(p-\- ep Uemh, (12) 
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where iXemb is the embedding input used to drive the system along any desired admissible trajectory. If we rewrite 
(12) in state space form as x^p = fip{t,Xcp,Uext): where x^p = (9^,9^) and Xqs = {(pqs^^qs): the following optimization 
problem may be posed 

minimize ^ \\x^{r) - Xqs{r)\\l^ + r\uemh{r)\^dr + ^\\x^{T) - Xqs{T)\\l^ ^^^^ 

subject to Xy, = /c^(t,Xc^,lAemb), ^(^(0) = Xqs{0). 

where we use the quasi- static trajectory as a desired curve to find the actual trajectory, Q^p > and P^p > are 
positive definite weighting matrices and r > is the weight of the embedding input. Using a sufficiently high weight 
r for the embedding input, we obtain a trajectory arbitrarily close to the exact lifted trajectory. The optimization 
problem is solved by using the projection operator based Newton method described in Appendix A. 

We denote LIFT^p the routine described above to solve the unconstrained lifting. Specifically, we let ^ = LIFT^p (^o5 yd(' 
be a trajectory solving the practical lifting task for the desired output curve yd(-) and computed by using as 
initial guess. The routine is parametrized by the coupling parameter ep of the PVTOL dynamics. For ep = the 
lifting procedure does not need any initial guess. Thus, we simply write ^ = LIFTo(yd(-))- 

Constrained L2 distance minimization and optimal control relaxation 

With a full unconstrained trajectory = (^dCO^ ^d(')) hand, we can pose the following constrained optimal 
control problem, where we minimize a weigthed L2 distance from subject to feasibility. 



min i r||a;(r)-Xd(r)||^ + i|mT)-«d(r)||^dr+i|KT)-Xd(r)|||. 

ix{-)M-)) Jo 



min h I 

subj. to x{t) = f{x(t)^u{t))^ x(0) = xo 

{x{t),u{t)) e for a.a. t G [0, T] 

where Q, R and Pf are positive definite matrices. The idea is to choose Q, R and Pf so that the weights associated 
to the outputs (external position states) are much larger than the weights of the other sates and the inputs. Denoting 

11^ - ^^^1^2 •= I lo ll^(^) ~ ^d(T)||Q + \\u{t) - Ud{r)\\% dr + ^\\x{T) - Xd(T)|||,^, we can rewrite the problem in a 
more compact notation as 

min ||^-^d||L2 

^^^^p (14) 

subj. to ^ G AfZY, 

where we have denoted Tip the trajectory manifold for a given value of the parameter ep . 

Now, we introduce a relaxed version of the above optimal control problem. To do that, we first define a relaxed 
version of the feasibility region. That is, we parametrize the feasibility region by means of a scaling factor p G [0, 1] 
that allows to enlarge the nominal region up to a larger one containing the unconstrained lifted trajectory £^d- 

Definition 5.1 (Scalable feasibility region) A scalable feasibility region is defined as 

'POAp = {(x, ^) G X I Cj (x, ^; p) < 0, p G [0, 1] , j G {1, . . . , k}} 

such that 

(i) Cj(x, p); j G {1, . . . , /c}; is C'^ in x and u and varies smoothly with p. 
(a) for any p G [0, 1]^ P^Up, the interior of XUp, is a nonempty simply connected set. 
(Hi) for any < pi < p2, P^^pi ^ ^^P2^' 

(iv) the projection of XUp on the input space TTuP^Up = {u G MJ^\{x^u) G P^Up V fixed x} is convex; 

(v) for every desired output trajectory yd('); lifted trajectory ^d = {x^{')^u^{')) (if it exists) is such that 3 po ^ 
[0, 1] such that {xd{t),Ud{t)) G P^Up^ for every t G [0,T]. 
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With this definition in hand we can introduce the relaxed version of the optimal control problem in (14). We use the 
barrier functional idea described in Appendix A. Namely, we add to the cost functional a barrier term to enforce 
feasibility with respect to the point- wise constraints. Thus, the optimal control problem relaxation is given by 

^nnn U - ^ e,bs,AO (15) 

where Tep is the trajectory manifold for a given value of ep and &5c,p(0 ^ barrier functional defined consistently 
with (A. 3). It is worth noting that here the barrier functional is parametrized also by p because the constraints are. 
The relaxed optimal control problem is solved by using the projection operator Newton method in Appendix A. 

Next, we introduce some useful notation. For a given scalable feasibility region A'ZYp, parametrized by we denote 
PO_Newtgp (<^d,^o; p) a routine that takes as inputs a desired (state-control) curve ^ind an initial trajectory ^o, 
and computes a feasible trajectory ^ G T^p , with ^ G ^Up, by solving the nonlinear optimal control relaxation in 
(15). The routine is parametrized by the embedding parameter ep (the coupling parameter for the PVTOL), the 
parameter Cc scaling the barrier functional (see Appendix A. 3) and the parameter p scaling the feasibility region. 

We are now ready to define our strategy. First, we provide an informal description. From now on we call unconstrained 
lifted trajectory a trajectory solving the practical (unconstrained) lifting task. 

Lift and Constrain Strategy: The strategy consists of the following steps: (i) given a desired output curve yd{')^ 
unconstrained lifted trajectory for an initial embedding system is computed (e.g., for the decoupled PVTOL with 
=0); (ii) a continuation update on the parameter ep is applied up to the nominal value ep°™; (iii) a relaxed 
optimal control problem is solved for p = (feasibility region containing the lifted trajectory) and = (for a 
suitable eco > 0); (iv) a continuation update on the parameter p is applied shrinking the feasibility region up to 
its nominal value for p = 1; (v) a continuation update on the parameter Cc is applied to regulate the closeness of 
the feasible trajectory from the boundary (cc e^e for some Ccg)- 

Next we give a pseudo-code description of the strategy. 



Strategy : 
Task: 
Inputs : 

Output : 
Parameters : 

Initialization : 



Lift and Constrain Strategy 
Feasible trajectory lifting 

ic ^ 7^^°™ with ^ 
(ep,ee,p) GM|o 

ep := 0, ec := eco, p := 
Cd :=LIFTo(yd(-)) 
:= U 



1. WHILE ep < ep°"^ DO 

increase ep 

ed = LiFT,p (ed;yd(-)) 

END 

2. WHILE p < 1 DO 

increase p 

= PO_Newtep (^d, ^c; ec, p) 

END 

3. WHILE ee > e^e DO 

decrease 

^c = PO_Newtep (^d,^c;ec,p) 
END 

4. RETURN 
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Remark 5.2 (Variations of the strategy) Other variations of the above strategy can be obtained by changing 
the order of or combining the update steps of some parameters in the continuation strategy. These different choices 
can be thought as degrees of freedom in the designer's hands and their effectiveness is strongly related to the system 
dynamics and to the feasibility constraints. 

6 Strategy analysis 

In this section we prove that under suitable conditions on the feasibihty region we can find a feasible trajectory that 
solves locally the practical lifting task in Definition 3.6. 

6. 1 Differentiability of an optimal control minimizer with respect to parameters 

We start providing a supporting result to prove the existence of a feasible trajectory. Namely, we prove, under 
suitable conditions, continuity and differentiability of an optimal control minimizer with respect to parameters. We 
present this result in a separate subsection for two reasons. First, this is the most subtle part to prove the existence 
of a feasible trajectory. Second, we believe this is an important stand alone result. 

We consider an optimal control problem where the cost functional depends smoothly on a finite dimensional param- 
eter and the system is independent of the parameter. In this section, we will refer to this parameter as p eW. This 
parameter may include, for instance, the scalar parameter p used for specifying the size of the feasible region as well 
as the scalar parameter Cc used in determining strictly feasible trajectories. We thus write the minimization problem 

min/i(^, p). 

Using Lemma A. 2 in Appendix A, we can look for an unconstrained local minimum of the functional 

We will suppose that the scaling and off"set of the parameters have been chosen in such a manner that the nominal 
value of the parameter vector is p = 0. 

Remark 6.1 (Parametrization with respect to system parameters) The parameter p does not include the 
system parameter ep . Indeed, the parameter Cp affects the dynamics and, thus, the projection operator V. The 
following results hold true for the case where also the projection operator depends on the parameter, i.e. gp{C) — 
h{V{^, p), p), provided the projection operator is shown to depend smoothly on the system parameter. 

Let ^0 G T and consider the nature of ^o(0 a neighborhood of ^o- In particular, we consider ^ of the form + C 
where ||C|| < S and ^ > is such that + C ^ dom V for each such For 9o{')^ we have 

5o(6 + C) = 50(^0) + Dgoi^o) ■ C + ^^'50(^0) • (C, C) + K^o, C) • (C, C) (16) 
where the remainder satisfies 

K^o,C)-(C,C)l/llCf ^0 as IICII^O (17) 
where || • || is the L^o norm. Using the identity 

0(1) = 0(O) + 0'(O)+ / {l-s)4>\s)ds 
Jo 

together with (j){s) = ^o(Co + -^O^ we obtain the explicit expression 

r-(6,C) • (Ci,C2) = [\l - s) [D^goi^o + <) - D^goi^o)] ds ■ (Ci,C2) (18) 

which has been slightly generalized to depend on three, possibly independent, perturbations. Using the fact that 
D^^o(') is continuous as a mapping from the trajectory manifold T to set of continuous bilinear functionals on Lqo, 
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we easily verify that the remainder r(^0 7 C) ' (C? C) defined by (18) satisfies, as it must^ the higher order property (17). 
Equations (16), (18) provide a second order expansion with remainder formula for the mapping go{'): valid in an 
Loo neighborhood of any ^ In fact, the formula given by (16), (18) is somewhat more general, requiring only 
that ^0 ^ ^oo and ^ > are such that ^^(^o) C dom V. 

Now, since the functional ^o(') is the composition of an integral functional and a projection operator, from [10] the 
value of the bilinear expression D^^o(0 ' (Ci? C2) for ^ ^ dom V and Q G I/oo is of the form 

D^goiO ' {C1X2) = r MrfW{T)^2{r) ^ir + (^171 (T))^Pj (^172 (T)), 
Jo 

where 7^ = DV{^,) • d and where Pf = Pj e M^^''' and the bounded matrix W{t) = W{t)'^ G W'''', t G [0,T], 
depend continuously on 77 = P(^), hence continuously on ^. Using these facts, we see that 

Lemma 6.2 Let ^0 ^ T and suppose that S > is such that Bs{^o) C dom V. Then, there is a nondecreasing 
function f(-) with f(0) = such that 

|r(eo,C)-(Ci,C2)|<r-(||C||)||Ci||Lj|C2||L. (19) 

for all CX1X2 e Bs. 

Proof: Since 7^ is C^, DV{i) is a continuous linear projection operator with respect to the Loo norm. Using an 
explicit formula for DV{^) • it can be shown, [10], that DV{£) may be extended to a linear projection operator 
DV{i)L2 ^2 that is continuous with respect to the L2 norm. The result follows easily using (18). ■ 



Suppose now that ^0 ^ T is a stationary trajectory of ^o(') so that L)^o(^o) ' C = for all Q G Loo and that ^ > is 
such that ^^(^o) C dom V. It follows that 

5o(?o + C) > 5o(Co) + \D^g,{io) ■ (C, C) - r-(llCll) llCllL (20) 

for all II ell < 5 where f(-) is given by Lemma 6.2. Restricting (20) to (" ^ ^io^^ obtain the fundamental second 
order sufficient condition (SSC) for to be an isolated local minimizer. 

Theorem 6.3 Suppose E T is such that D^o(^o) ' C = for all ( G Loo CLnd that there is a cq > such that 

£''5o(Co)-(C,C)>co||CllL forallCeT^,r. (21) 
Then isolated local minimizer in the sense that there is a S > such that 

9o{io) <go{0 

for aineT with lie-^oll e^^eo. 

Proof: Taking > be such that f{5i) < co/4, we find that ^0(^0 + C) > ^0(^0 ) + (co/4)||C||i2 C^T^o^ with 

lie II < ^1. By Theorem A.l, each ^ G T near ^0 can be represented by a unique (" G T^qT according to ^ = 7^(^o + C) 
and the mapping ^ 1-^ is continuous. Thus there is a (5 < (5i such that ||^ — ^o|| < ^ implies that ||C|| < 5i. The 
result follows. ■ 



We call a local minimizer ^0 ^ 7" satisfying (21) a second order sufficient condition local minimizer^ SSC local 
minimizer for short. According to Theorem 6.3, every SSC local minimzer is an isolated local minimizer. We also 
note that, in words, the condition (21) says that the quadratic functional ^ 1-^ L)^^o(^o) • (C? C) is strongly positive 
on the subspace T^^T. 

Consider now the (local) minimization of gp{i) as the parameter p is varied on a neighborhood of p = where ^0 is 
known to be an SSC local minimizer of ^o(0- Since D'^gp{^) is continuous in both ^ and p, we expect that, for each 
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sufficiently small p, there will be a corresponding SSC local minimizer near and that the mapping p ^ will 
be continuous, and perhaps differentiable. The key idea is to use an appropriate implicit function theorem (IFT) to 
solve the first order necessary condition equation 

DgpiQ = (22) 

for ^p as a function of p starting from at p = 0. Proceeding formally, we differentiate (22) with respect to p to 
obtain 

Thus, the derivative of ^p with respect to p, (^^, if it exists, is given formally by 

^'^ = -[D{Dg,{^,)}]-'.^^Dg,{^,). 

In this case, we expect that there is an implicit function theorem that says something like, if D{Dgp{^p)} is invertible 
at p = 0, then there is a neighborhood of p = on which p i-^ is well defined and C^. In what sense should the 
operator D{Dgo{£^o)} be invertible and how can it be ensured? It turns out that the appropriate condition is that 
i^^^o(Co) be strongly positive on T^qT, i.e., that it satisfy (21). 

Theorem 6.4 Suppose that ^ T is an SSC local minimizer of goiO- Then, there is a S > such that, for 
each p such that \\p\\ < S, there is a local SSC minimizer ^p of gp{C) near ^o- Furthermore p ^ ip is continuously 
differentiable. 

Proof: The key is to show that, for p sufficiently small, we can compute a ^ G T such that Dgp{^) • ^ = for 
all C G LoQ. Using again Theorem A.l, we proceed by parametrizing ^ G T locally by 7 G T^^T according to 
i = "^(^0 + 7) and searching over 7. As in the proof of many IFTs, we solve for the desired 7 using a contraction 
mapping. For simplicity, we will denote T^qT by X so that we search for 7 G X such that i^5'p('P(^o + 7)) • C = for 
aU C G Loo. 

First, note that, since D^^o(^o) is strongly positive on X, the well-defined quadratic minimization problem 

A = arg min -cj • C + ^i^^^o(^o) • (C, C) 

defines a linear mapping SB \ uj ^ \ \ dom SB C X* X for some continuous linear functional cj G X*. The 
linear mapping BB provides the solution A G X to the functional equation 

D^go{^o)-{X,0=OJ-C, CeX, 

effectively providing an inverse to the operator D{i^^o(Co)} formally described above. We will see that the functionals 
(J G X* of interest belong to the domain of BB. 

Define JT, : X ^ X* by 

Tp{j) . C = D^goi^o) • (7, C) - Dgpi^o + 7) ' C 
for all G X. Note that J-pij) • C is of the form 

J'ph) C = r <rf4r) + b{rfv{T) dr + r^ z{T) 
Jo 

for ( = {z{-)^v{-)) G X where a(-), 6(-), and ri depend smoothly on the data p and 7. It follows that J^pi'j) G 
dom BB C X*. A straightforward calculation shows that 

gp(7) = 55-7-p(7) 

defines a continuous operator Qp : X ^ X that is also continuous in p. 
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Note that, if 7 G X is a fixed point of ^p(-), 7 = ^^(7), then Dgp{£^o + 7) • (" = for all ^ X. This will imply that 
Dgp{£,o + 7) • C = for all C G Loo provided that V{£,o + 7) is sufficiently near ^o- Iii that case, we conclude that 
= ^(^0 + 7)- Also, for p = 0, we see that 7 = is the fixed point, ^o(O) = as expected. 

We will show that, for p sufficiently small, Gp{') is a contraction mapping with a unique fixed point. For p = 0, 
noting that Dgp{^o + 7) • (•) = £^^^0(^0) • (7, ') + ^(hll), we see that 

^0(7) = SS . {D'goiCo) ' (7, •) - Dg.iCo + 7) ' (O) = ^(hll) 

where we have used the fact that SS is continuous (bounded) on the elements of X* of the noted form. By continuity 
in we see that there exist pi,5 > such that 

11^.(7)11 

whenever ||p|| < pi and ||7|| < S. Now, fixing p, ||p|| < pi, 

Gpili) - GpM = SS • [Dg.iCo + 72) • (•) - Dg.iCo + 7i) ' (•)] 



= SS- 

so that there is a A: < cx) such that 



/ D'^9p{^o + 71 + <5(72 - 71)) ds • (72 - 7ir) 
Jo 



\\gpM-Qph2)\\<k5\\^i-72\\ 

for II71II < S and II72II < 6. Shrinking S, if necessary, so that kS < 1/2, we see that Qp is a contraction with unique 
fixed point jp. 

To see that p>-^ jp is continuous, write 

lllpi -7p2ll = WOpiilpi) -0p2{'yp2)\\ 

< WGp.M-Gp.MW + WQp.M-Gp^MW 

< (l/2)||7p. -7p.ll + WQp^M-QpMW 

SO that 

||7pi-7pJI <2||ap,(7pJ-^P.(7pJII 
showing that p i-^ 7^ is continuous since p 1-^ Gpil) is continuous (for fixed 7). 

Differentiability is proven following standard arguments from implicit function theorems applied to Qp. See, e.g., the 
second part of the proof of Theorem 4.E in [31]. ■ 



6.2 Existence of a feasible lifted trajectory 

With the continuity result of the previous subsection in hands, we can prove our existence result. For a general 
maneuvering system we make the following standing assumption. 

Assumption 6.5 Let yd{t), t G [0,T]^ be a given desired output curve. There exists a (state-input) trajectory 
U = (xd(-),'^^d(-)) on [0,r], such thatp{xd{t)) = yd(t) for all t e [0,T]. 

Formally, we state our main result in the next theorem. 

Theorem 6.6 (Existence of a feasible lifted trajectory) Let yd{t), t ^ [0?^]; ^ desired sufficiently smooth 
output curve satisfying Assumption 6.5 and XU C x a compact feasibility region. Then, the following holds 
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(i) for a given 5^ > 0; there exist po > ci'^d ^co > such that the problem 

mmU-U\\L,+eM,AO. (23) 

has an isolated local minimizer for all < p < po and < Cc < Cco; 
(a) if is an isolated local minimizer of the problem in (23) for given p > and > 0, then there exists 62 > 
such that 

r-^diu. < u-u\L.+e2 

for all trajectories ^ £ T in a neighborhood of ; 
(Hi) for ^* as in (ii), then there exists €3 > such that 

lb(^*(-)) - YdOIU. < IbWO) - YdOIU. + £3 

for all trajectories ^ = e T in a neighborhood of G T- 

Proof: Statement (i) is just a straightforward corollary of Theorem 6.4. To prove statement (ii), we observe that 
from (i) there exists 5 > such that 

lir - Uh, + < U - Uh, + eMO 

for all ^ G -B(r,^), so that 

r - ?<ilU. < lie - CdlU, + ec(6p(0 - MD)- 
Exploiting the structure of bs^^p and using the linearity of the integral operator we can write 

bsUO-bsUC) = r ^/3{-cMr),n{T))) - p{-c,{x*{T),u*{T)))dT 
Jo J 

Using the fact that ^5 is a function and each cj is in both arguments (so that they are all bounded on [0,T]), 
there exists C2 > such that 

bsUO-bsUn < T m^x Y,l3i-cMr)Mrm - l3{-Cj{x*{T),u*{T))) < Tc^. 



The result follows by choosing €2 = ecTc2- 

To prove statement (ii) we assume, without loss of generality, that the state vector can be written as x = [xj x^]^ ^ 
where xi G MP is the vector of performance outputs, that is p{x) = xi, and X2 G M^~^ the remaining portion of the 
state. We use the same partition for any (state-input) curve so that, given a desired curve G X, a weigthed L2 
distance of a curve ^ G X from a lifted trajectory id satisfies 

lie-^dllL, <| riki(T)-Xid(r)|||, + ||a;2(r)-a;2d(T)|||, + |Kr)-t^d(r)|||dr 

^0 (24) 

+ \\\x^{T) - xUml, + i||x2(T) -:r2d(T)||2,^ +C3 

where Qi, Q21 Pi and P2 are positive definite matrices and C3 > is a positive constant taking into account cross 
terms. Now, rearranging terms in (24), we have 

U - Ul, < \ r Iki(r) - x,^{T)\\l^dT + i||xi(r) - a;id(T)|||,^ + 
^0 

i / ||x2(r)-a;2d(T)|||, + i||a;2(T)-X2d(T)||2,^ + i / ||«(r) - «d(r)||| dr + C3 
Jo Jo 

= \\P{X{-)) - yd(-)l|L2 + \\x2i-) - X2d{-)\\L, + \\U{-) - Wd(-)I|L2 + C3, 
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where we have used the fact that is a hfted trajectory (and thus xid(-) := p{xd{-)) = yd(')) supposed that 
the weighted L2 norm for the input has no terminal penalty. Using similar arguments, the converse inequality can 
be obtained, i.e., U - U\\l2 > l|p(^(-)) - yd(-)IU2 + Ik2(-) - ^2d(-)IU2 + ll^(') " ^dCOIUs -C4, for a suitable C4 > 0. 
It is worth noting that, if the weight matrices Q, R and Pf are block diagonal the above inequalities are equalities 
with C3 = C4 = 0. 

From (ii), we have that there exist ^* G T and €2 > such that ||^* — ^^Uls ^ ||^ — ^dlUs + ^2 for all ^ G T in a 
neighborhood of Thus, we can write 

- yd(-)l|L. + \\xli-) - X2d(-)I|L. + \\U*{-) - U^{-)\\l. < 

- yd(-)IU. + \\x2i-) - ^2d(-)IU2 + wo - M-)\\l, + 62 + C3 + C4, 

and, 

IW^*(0)-yd(OllL.<bWO)-yd(OllL^ 

{\\X2{') - X2d{')\\L, - Mi') - X2d{')\\L, + ll^(-) - Ud{')\\L, " ||^*(-) " ^d(-)IU. + 63 + C3 + C4). 

Using the same arguments on boundedness of the state and input trajectories and boundedness of the weighted 
1/2 norm on a neighborhood of ^* as in (ii), we have that there exists > such that ||p(x*(-)) — yd(-)IU2 — 
||^(x(-)) - yd(-)IU2 + ^3, thus concluding the proof. ■ 



Remark 6.7 (Analysis of the lift and constraint strategy for the PVTOL) The initialization part of the 
lift and constrain strategy for the PVTOL can be easily performed. Indeed, the LIFTq procedure simply implements 
equations in (4). Then we proceed by analyzing each step. As for Step 1, we can use Theorem 4-6 to prove that 
there exists epo > such that for any ep < epo an unconstrained lifted trajectory for the coupled PVTOL exists 
and depends continuously on the coupling parameter. Thus, we know that for "smalV^ positive values of the coupling 
parameter the continuation procedure will be successful, thus providing an unconstrained desired trajectory for the 
following continuation steps. Using Theorem 6.6 (i), we have that for feasibility regions that are not too tight Step 2 
and Step 3 will be successful and, thus, a feasible trajectory that locally solves the practical feasible lifting task can 
be found. 



7 Numerical computations on the PVTOL 

In this section we present numerical computations showing the effectiveness of the proposed strategy on the PVTOL. 
We proceed defining the feasibility region (equivalently the point- wise constraints that the system trajectories must 
satisfy) and the desired maneuver. Regarding the feasibility region, we consider the case of constraining the input 
only, i.e. ui and U2 are bounded with, in particular, ui strictly positive. In order to have a compact feasibility region 
we just assume that the states must belong to a sufficiently large compact and simply connected set Q C such 
that the state portion of the trajectory is for sure feasible. The feasibility set is thus defined as 

XU = {(X, IX) G n X M^IO < Uimin <Ui < iXlmax, U2min < U2 < '?/2max for givCU 1/lmin, '^^Imax, '?^2min and 'i/2max}- 

Then, we parametrize the feasibility region with the scaling parameter p and define the inequalities determining the 
barrier functional. We pose the inequalities in terms of the square distance of ui and U2 from the boundary values 
in order to have a smooth barrier functional. The two inequalities are as follows 







U2 - 



('^2max + '^2min) 



< 



< 



{p^{l-p)k^) 



{p^{l-p)k2) 







('^2max ~ '^2min) 



where we set i^imin = 0.5^ {g being the gravity constant), i^imax = m/s^, 
deg/s^, while ki and k2 are two scaling factors that guarantee feasibility for p = 



^2min 

0. 



= -80 deg/s^, U2max = 80 
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Regarding the desired raaneuver, we aira at performing a barrel roll with a constant velocity profile. Specifically, we 
choose the desired outputs, yd{') and Zd{-), so that the desired path is the one depicted in Figure 3a and the velocity 
is Vd = 10m/ s. 

We are now ready to present the results of the main steps of the lift and constrain strategy for the described feasibility 
region and desired maneuver. A lifted trajectory for the decoupled PVTOL (ep = 0) is computed according to 
Equations (4). Recall that this trajectory is a quasi-static trajectory for any coupled PVTOL. Then, using the 
dynamic embedding technique described in Section 5, we compute a lifted trajectory for the coupled PVTOL with 
nominal parameter ep =1. We solve the optimal control problem by using the projection operator Newton method. 
The quasi-static versus lifted path is depicted in Figure 3 together with the velocity and roll trajectories. The 




(a) path (b) velocity (c) roll 



Fig. 3. Quasi-static (decoupled PVTOL) vs lifted (coupled PVTOL) path, velocity and roll trajectories for the unconstrained 
PVTOL (ep =1). Specifically: the dashed green lines are the quasi-static curves, while the solid blue are the lifted trajectories. 

decoupled versus coupled roll-rate and input trajectories are depicted in Figure 4. As appears from the picture, the 




(a) roll-rate (b) ui (c) U2 

Fig. 4. Quasi-static (decoupled PVTOL) vs lifted (coupled PVTOL) roll rate and input trajectories for the unconstrained 
PVTOL (ep =1). Specifically: the dashed green lines are the quasi-static curves, while the solid blue are the lifted trajectories. 

desired and lifted paths and velocities are indistinguishable (the maximum error is of order 10~^ consistent with the 
required absolute tolerance). This is consistent with the result in Theorem 4.6 stating that the lifting task can be 
solved exactly for some positive values of ep . As expected the roll and roll rate trajectories and the input trajectories 
are significantly different from the quasi-static ones. In particular, the optimal roll and roll rate trajectories display 
a degree of anticipation and are smoother than the quasi-static. This is due to the filtering action of the dynamics. 
This filtering effect can be seen also in the snapshots of the PVTOL animation in Figure 3a. It is worth noting that 
neither the desired nor the lifted trajectory satisfy the constraints and, thus, are both infeasible. 

Now, we are ready to show the results of the "constrain" part of the strategy. As regards the L2 weights, we choose 
diagonal Q and R matrices (for simplicity) and penalize the outputs (external position states) 10^ times the other 
states and the inputs. We initialize the strategy by setting = 10. We choose a relatively high value for so that for 
each given value of p the trajectory that we find is sufficiently far from the boundary. This has two advantages. First, 
when we increase p, thus shrinking the feasibility region, the constraints are only slightly violated if the step-size on 
p is not too large. Second, once p = 1 has been reached, we can converge to a tighter approximation (ec — ^ 0) in an 
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interior point fashion. The parameter p is varied with a step-size of 0.2. For each value of p the minimization takes 
few (less than 5) Newton iterations (with an absolute tolerance on the descent direction set to 10~^). Once reached 
the value p = 1, ec is decreased down to = 0.1. 

The desired versus feasible path, velocity and roll trajectories are depicted in Figure 5 (for p = I and = 0.1). 
Snapshots of the PVTOL animation are shown Figure 5a as for the unconstrained case. For the velocity and roll 
angle we also plot intermediate non-optimal trajectories obtained during the continuation procedure. In particular 
we plot the trajectories obtained for p = 0.6 and p = I with = 10. It is interesting to notice that even in presence 




(a) Path (b) Velocity (c) Roll 

Fig. 5. Desired vs feasible path, velocity and roll angle for the coupled PVTOL (ep = 1). Specifically: the dashed green 
lines are the desired infeasible curves, the solid blue lines are the feasible optimal trajectories (p = 1 and Cc = 0.1), and (for 
velocity and roll angle) the thinner dotted grey lines are intermediate trajectories obtained for p = 0.6 and p = 1 with Cc = 10 
respectively 

of tight constraints on the two inputs the feasible trajectory output (path and velocity) is reasonably close to the 
desired one. The maximum error on y{-) and z{-) is less than Im and the maximum error on the velocity is less than 
2m/s. Also, it is worth noting that the roll trajectory. Figure 5c, stays bounded and relatively close to the desired 
one even if the weight in the cost function is much lower than the one on the positions. 



In Figure 6 we show the feasible versus desired roll-rate and inputs for the same values of p and ec. The controls have 
a bang-bang like behavior. In particular they tend to assume the boundary value in a larger interval than the one on 
which the constraints are violated in order to compensate the missing control effort in the infeasible time windows. 
This non-causal behavior shows that, in order to obtain performances that are comparable with the unconstrained 
case the optimization needs to work in a non-causal fashion. 














( ' • 

































fa) roll-rate 



(b) ui 



(c) U2 



Fig. 6. Desired vs feasible roll-rate, ui and U2 for the coupled PVTOL (ep = 1). Specifically: the dashed green lines are the 
desired infeasible curves, the solid blue lines are the optimal trajectories (yo = 1 and Cc = 0.1), and the thinner dotted grey 
lines are intermediate trajectories (obtained for p = 0.6 and p = 1 with ec = 10 respectively. 



The results obtained in the above computations show that the inputs tend to be discontinuous when ec ^ thus 
suggesting that the constrained minimizer is, in fact, discontinuous. In order to have a smoother input we could 
vary the parameters of the strategy. We could, e.g., increase the parameter thus obtaining a smoother trajectory 
(as the intermediate feasible trajectory shown in Figure 6 in dotted grey line) or increase the L2 input weights (the 
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coefficients of the matrix R). Both the two procedures increase the error on the desired output. Next, we show a 
different procedure to obtain the same input regularization without loosing too much in terms of output error. This 
procedure ahows us to show that the proposed strategy can easily deal with both state and input constraints without 
any increase in the complexity of the strategy. We proceed by applying a dynamic extension to the system, [18]. In 
the extended system the actual inputs are two additional states, while their derivatives are the new inputs. 

X = /(x, w) 
w = 

where x = [y z (py z (f>] and w = [ui U2] are the states of the extended system and v = [iii U2] is the input. 

The results are shown in Figure 7. We compare the feasible path and original inputs {ui and U2) obtained with and 
without dynamic extension. In particular, for the extended system both the original inputs (additional states) and 
the original input derivatives (new inputs) are constrained. 




(a) Path (b) ui (c) U2 

Fig. 7. Desired vs feasible path and inputs for the coupled PVTOL (ep = 1) with and without bounds on the input derivatives. 
Specifically: the dashed green lines are the desired curves, the solid blue lines are the optimal trajectories for the system with 
dynamic extension (bounds on both inputs and input derivatives) and the dotted grey lines are the optimal trajectories for 
the system without dynamic extension (bounds on the inputs only). 



8 Conclusions 

In this paper we have studied a constrained trajectory lifting problem for nonlinear control systems. Given a desired 
output curve for a nonlinear maneuvering system, we compute a full (state- input) trajectory of the system such that: 
(i) the output portion is close to the desired one, and (ii) a set of point-wise state-input constraints are satisfied. 
We have proposed a nonlinear optimal control strategy based on a novel projection operator based Newton method 
for point- wise constrained control systems [10,12] combined with dynamic embedding, constraints relaxation and 
continuation with respect to parameters. Under suitable values of the system and constraints parameters we have 
proven that a feasible trajectory exists and can be computed by means of the proposed strategy. Finally, we have 
completely characterized the strategy for the PVTOL aircraft and provided numerical computations showing the 
effectiveness of the strategy for an aggressive desired barrel roll maneuver in presence of relatively tight constraints. 

References 

[1] S. A. Al-Hiddabi and N. H. McClamroch. Tracking and maneuver regulation control for nonlinear nonminimum phase systems: 
application to flight control. IEEE Transactions on Control Systems Technology^ 10(6), 2002. 

[2] S. Boyd and L. Vandenberghe. Convex Optimization. Cambridge University Press, 2004. 

[3] A. Isidori C. I. Byrnes. Output regulation for nonlinear systems: an overview. International Journal of Robust and Nonlinear 
Control, 10:323-337, 2000. 

[4] L. Consohni, M. Maggiore, C. Nielsen, and M. Tosques. Path following for the PVTOL aircraft. Automatica, 46:1284-1296, 2010. 

[5] L. Consolini and M. Tosques. On the VTOL exact tracking with bounded internal dynamics via a poincare map approach. IEEE 
Transactions on Automatic Control, 52:1757-1762, 2007. 

[6] S. Devasia. Approximated stable inversion for nonlinear systems with nonhyperbolic internal dynamics. IEEE Transactions on 
Automatic Control, 44(7): 1419-1425, July 1999. 



20 



[7] S. Devasia, D. Chen, and B. Paden. Nonlinear inversion-based output tracking. IEEE Transactions on Automatic Control, 41(7):930- 
942, July 1996. 

[8] S. Devasia and B. Paden. Stable inversion for nonlinear nonminimum-phase time-varying systems. IEEE Transactions on Automatic 
Control, 43(2):283-288, February 1998. 

[9] N. H. Getz and J. E. Marsden. Control for an autonomous bicycle. In IEEE Int. Conf. on Robotics and Automation, volume 2, 
pages 1397-1402, May 1995. 

[10] J. Hauser. A projection operator approach to the optimization of trajectory functionals. In IFAC World Congress, Barcelona, 2002. 

[11] J. Hauser and D. G. Meyer. The trajectory manifold of a nonlinear control system. In IEEE Conf. on Decision and Control, 
volume 1, pages 1034-1039, December 1998. 

[12] J. Hauser and A. Saccon. A barrier function method for the optimization of trajectory functionals with constraints. In IEEE Conf. 
on Decision and Control, pages 864-869, San Diego, Dec 2006. 

[13] J. Hauser, A. Saccon, and R. Frezza. Aggressive motorcycle trajectories. In IFAC Symposium on Nonlinear Control Systems, 
Stuttgart, 2004. 

[14] J. Hauser, A. Saccon, and R. Frezza. On the driven inverted pendulum. In IEEE Conf. on Decision and Control and European 
Control Conference, pages 6176-6180, Dec. 2005. 

[15] J. Hauser, S. Sastry, and G. Meyer. Nonlinear control design for slightly nonminimum phase systems: Application to V/STOL 
aircraft. Automatica, 28(4):665-679, 1992. 

[16] J. Huang. Nonlinear output regulation. Theory and applications. SIAM, Philadelphia, 2004. 

[17] L.R. Hunt and G. Meyer. Stable inversion for nonlinear-systems. Automatica, 33(8):1549-1554, 1997. 

[18] A. Isidori. Nonlinear Control Systems. Communications and Control Engineering Series. Springer, 3 edition, 1995. 

[19] A. Isidori and C.I. Byrnes. Output regulation of nonlinear systems. IEEE Transactions on Automatic Control, 35(2): 131-140, 
February 1990. 

[20] A. Isidori, L. Marconi, and A. Serrani. Robust Autonomous Guidance. An Internal Model Approach. Advances in Industrial Control. 
Springer, 2003. 

[21] L. Marconi, A. Isidori, and A. Serrani. Autonomous vertical landing on an oscillating platform: an internal model based approach. 
Automatica, 38:21-32, 2002. 

[22] H. Maurer and H. J. Pesch. Solution differentiability for nonlinear parametric control problems. SIAM Journal on Control and 
Optimization, 32(6):1542-1554, 1994. 

[23] H. Maurer and H. J. Pesch. Solution diflPerentiability for parametric nonlinear control problems with control-state constraints. 
Journal of Optimization Theory and Applications, 86(2):285-309, 1995. 

[24] R. Naldi and L. Marconi. Optimal transition maneuvers for a class of V/STOL aircraft. Automatica, 47, 2011. 

[25] G. Notarstefano, J. Hauser, and R. Frezza. Computing feasible trajectories for control- const rained systems: the PVTOL aircraft. In 
IFAC Symposium on Nonlinear Control Systems, Pretoria, SA, August 2007. 

[26] G. Notarstefano, J. Hauser, and R.Frezza. Trajectory manifold exploration for the PVTOL aircraft. In IEEE Conf. on Decision 
and Control and European Control Conference, pages 5848-5853, Seville, December 2005. 

[27] A. Pavlov, N. Van de Wouw, and H. Nijmeijer. Global nonlinear output regulation: Convergence-based controller design. Automatica, 
43:456-463, 2007. 

[28] A. Pavlov and K. Y. Pettersen. A new perspective on stable inversion of non-minimum phase nonlinear systems. Modeling, 
Identification and Control, 29(l):29-35, 2008. 

[29] P.Martin, S. Devasia, and B. Paden. A different look at output tracking: control of a VTOL aircraft. Automatica, 32(1): 101-107, 
1996. 

[30] M. W. Spong and D.J. Block. The pendubot: a mechatronic system for control research and education. In IEEE Conf. on Decision 
and Control, pages 555-556, December 1995. 

[31] Eberhard Zeidler. Applied Functional Analysis: Main Principles and their applications. Springer- Verlag, New York, 1995. 

A The Projection Operator approach for the optimization of trajectory functionals 

In this section we recall the main mathematical tools that we use to develop the feasible trajectory exploration 
strategy and to prove its correctness. 

A.l Trajectory tracking projection operator 

The trajectory tracking projection operator, [11], provides a numerically robust representation of nonlinear system 
trajectories and is at the basis of the novel descent methods for nonlinear optimization of trajectory functionals, 
[10], used in the paper. Let us consider the nonlinear control system x{t) = f{x{t)^u{t)), x{0) = xq, where f{x^u) 
is a map in x G and u G . We recall that a hounded curve 77 = is a (state-input) trajectory 
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of the system if x{t) = f{x{t)^u{t))^ x(0) = xq, for all t G [0,T], < T < +00. Suppose that ^(t) = {a{t) , jii{t)) , 
t G [0,T], is a bounded curve (e.g., an approximate trajectory of the system) and let r]{t) = t G [0,T], 

be the trajectory determined by the nonlinear feedback system 

x{t) =f{x(t),u(t)), x(0) =xo, 
u{t) =fi{t) + K{t){a{t) - x{t)) 

Under certain conditions on / and this feedback system defines a continuous, nonlinear projection operator 

V := [a, fi) ^ T] = (x, u). 

That is, independent of if ^ is a trajectory, ^ G T, then is a fixed point of P, ^ = V{i). If K is bounded (and, if 
^ is a trajectory of infinite extent, such that the above feedback exponentially stabilizes ^0)5 then V is well defined on 
an Loo neighborhood of ^0 ^ind is (with respect to the L^q norm) on its domain (including an open neighborhood 
of ^0) whenever / is [10]. The first derivative of the projection operator, C, ^ DV{C} ' is the (continuous) linear 
projection operator given by the standard linearization 

z{t) = Airjit))z{t) + Bin{t))v{t), z{0) = 0, 
v{t) = iy{t) + K{t)[m - z{t)] . 

where • C = (z{-),v{-)), with C = and A{rj{t)) = f^{x{t)Mt)) and B(r;(i)) = Ux{t),u{t)). The 

tangent space T^T a given trajectory ^ G T is, thus, the set of curves satisfying = DV{C) • C- 

The projection operator V provides a convenient parametrization of the trajectories in the neighborhood of a given 
trajectory [11]. Indeed, the tangent space T^T can be used to parameterize all nearby trajectories. 

Theorem A.l (Trajectory manifold representation theorem [11]) Given ^ G T; there is an e > such that, 
for each rj E T with \\r] — C\\loo < ^ there is a unique ^ G T^T such that rj = V{$, + C)- This provides a atlas of 
charts, indexed by trajectories ^ eT, so that T is a Banach manifold. 

A. 2 Projection operator based Newton method 

Consider the unconstrained optimal control problem 

minimize / 1{t, x{r)^u{r))dr -\- m{x{T)) 

Jo (A.l) 

subj. to x{t) = f{x{t)^u{t))^ x(0) = xq. 

where l{t,x,u) is in x and convex in and in t, and m{x) is in x. This problem is equivalent to the 
constrained optimization problems 

min/i(0 = niin h(^). 

where h{^) := 1{t^ x{t)^ u{T))dT-\-m{x{T)) and the constraint set, the trajectory space T, is a Banach submanifold 

of X = (xq, 0) + -^oo- Next lemma is the basis for the Projection Operator Newton descent method that we use in 
our strategy. This result is also useful to prove the existence of a feasible lifted trajectory. 

Lemma A. 2 (Unconstrained minimization through projection [10]) Let ^(^) := h{V{i)), for ^ E U d X 
with Viti) CU C dom V . Then, the optimization problems 

min/i(0 and minofO 

are equivalent in the following sense. If eTCiU is a constrained local minimum of h, then it is an unconstrained 
local minimum of g. If G is an unconstrained local minimum of g in U, then ^* = P(^^) is a constrained local 
minimum of h on T. 
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The projection operator based Newton method, [10], is the foUowing. 



Algorithm (projection operator Newton method) 
Given initial trajectory ^ T 
For i = 0,1,2... 

design K defining V about 

search direction: 

Ci = arg min Dflte) ■ C + lo^gi^C, C) 

Step size: 7^ = argmin^e(o,i] ^(^ + 70); 
project: ^^+1 = V{^i + 7^0)- 
end 



It is worth noting that the two main steps of designing the K and searching for the descent direction involve the 
solution of suitable (well known) LQ optimal control problems. 

A. 3 Barrier functional approach for constrained optimal control 

Next, we present an interior point method, introduced in [12], for the optimization of trajectory functionals in 
presence of point-wise state and input constraints. The objective is to solve over the class of bounded inputs the 
optimization problem (A.l) subject to the point- wise inequality constraints 

Cj{t,x{t),u{t)) <0, j e {1,...,A:}, for almost all t e [0,T], 

where Cj{t,x,u) is in (x^u) and in t. The main idea proposed in [12] is to approximate the solution of the 
constrained problem by solving an unconstrained optimal control problem through a suitable translation of the well 
known barrier function method used in finite dimension convex optimization [2]. The direct translation to infinite 
dimension would be 

minimize / l{r,x{r),u{r)) — ec/^log{—Cj{t,x{T),u{T)))dT-\-rn{x(T)) 

Jo ^ (A.2) 

subj. to x{fy = f{x{t),u{t)), x{0) = xq 

A key difficulty of the problem in (A.2) is that the cost functional can not be evaluated at infeasible curves. The 
problem is resolved by introducing the approximate barrier function ^^^(•), < < 1, defined as 



■ log z z > Sc 



k-1 
k 



( z-kSc V 



The associated barrier functional is 



^^c(0= rj2f^^ci-Cj{r,c,{r),Krm (A.3) 
Jo J 

which is well defined for any curve ^ G X, so that we get the optimal control problem relaxation 

min/i(0 + ee&5^(0- 

Remark A.3 (Projection operator Newton method to solve the relaxed problem) The projection opera- 
tor Newton method can be used to optimize the functional QecsdO — ^(^(0) + ^c^(5c(^(0) P^'^ ^ continuation 
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method on the parameters Cc and Sc. The technique is to start with a large Cc and 5c, solve the problem mingecsdO 
using the Newton method starting from the current trajectory and then reduce Cc and Sc. It is worth noting that for a 
fixed €c it is possible to iterate on Sc up to a value for which the solution is the same as the pure barrier functional. 
For this reason in the paper we neglect the dependence of g^^^sdO (thus writing gedO)- 
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